#include <uav.h>
#include <iostream>
#include <cstdio>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "uav_controller_node");
    ros::NodeHandle nh("");
	ros::Rate rate(20);

    int uav_number;
    int ctrl_type=2;
    float (*hpos)[3];
    float x,y;

    nh.param<int>("uav_number", uav_number, 3);
    hpos = new float[uav_number][3];
    for (int i=0;i<uav_number;i++){
        nh.param<float>("x"+ std::to_string(i), x, 0);
        hpos[i][0]=x;
        nh.param<float>("y"+ std::to_string(i), y, 0);
        hpos[i][1]=y;
        hpos[i][2] = 0.0;
    }


    Uav *uavs = new Uav[uav_number]();
    for(int i = 0 ;i < uav_number; i++)
    {
        uavs[i].Setup(i,nh, rate);
        uavs[i].home(hpos[i][0],hpos[i][1],hpos[i][2]);
        uavs[i].init();
    }

 
    std::cout<<"success"<<std::endl;
    // std::cout<<"please choose control type: (1 for postion / 2 for velocity)"<<std::endl;
    // std::cin>>ctrl_type;
    // std::cout<<"type:"<<ctrl_type<<std::endl;

    while(ros::ok())
    {   
        for (int i=0;i<uav_number;i++){
            uavs[i].enable_armed();
            uavs[i].enable_offboard();
            
            // position ctrl
            if (ctrl_type==1){
                uavs[i].fly_2_target();
            }

            // velocity ctrl
            else if (ctrl_type==2){

                uavs[i].velocity->linear.x = 0.0;
                uavs[i].velocity->linear.y = 0.0;
                uavs[i].velocity->linear.z = 0.0;
                for (int j=0;j<uav_number;j++){
                    if (i!=j){
                        uavs[i].add_APF_barrier(uavs[j].nowpose->pose.position.x + hpos[j][0] - hpos[i][0],
                                                uavs[j].nowpose->pose.position.y + hpos[j][1] - hpos[i][1],
                                                uavs[j].nowpose->pose.position.z + hpos[j][2] - hpos[i][2]);
                    }
                }
                uavs[i].add_APF_cmdpose();
                uavs[i].add_APF_scan();
                uavs[i].send_APF_cmd();
                // uavs[i].send_vel_cmd(0,0,0);
            }

            ros::spinOnce();
            rate.sleep();
        }

    }

    return 0;
}

